#ifndef __AUGMAVCONN__
#define __AUGMAVCONN__

#include <boost/bind.hpp>
#include <boost/signals2.hpp>
#include <boost/smart_ptr.hpp>
#include <boost/make_shared.hpp>
#include <boost/system/system_error.hpp>
#include <memory>
#include <mavconn/interface.h>

#include <ros/ros.h>

using namespace mavconn;
class FiFoConnector;
//using namespace mavlink;
namespace augmavconn{

inline uint8_t getMavMsgId(const mavlink_message_t *message){
    return message->msgid;
}
/**
 *some issue with the length:
 *actually, the mavlink_message_t has a certain length:
    MAVPACKED(
    typedef struct __mavlink_message {
            uint16_t checksum; ///< sent at end of packet
            uint8_t magic;   ///< protocol magic marker
            uint8_t len;     ///< Length of payload
            uint8_t seq;     ///< Sequence of packet
            uint8_t sysid;   ///< ID of message sender system/aircraft
            uint8_t compid;  ///< ID of the message sender component
            uint8_t msgid;   ///< ID of message in payload
            uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8];
    }) mavlink_message_t;

  * the payload 64 has a certain length, when making buffer, we need a length of sizeof(mavlink_message_t)
*/
inline int getMavMessageLen(const mavlink_message_t *message){
    return (int)(sizeof(mavlink_message_t));
}


namespace sig2 = boost::signals2;
class AugMavConnInterface:public boost::enable_shared_from_this<AugMavConnInterface>{
    private:
	AugMavConnInterface(const AugMavConnInterface&) = delete;
    
    public:
    typedef sig2::signal<void(const mavlink_message_t *message, uint8_t system_id, uint8_t component_id)> MessageSig;
    typedef boost::shared_ptr<AugMavConnInterface> AugPtr;
    //using Ptr = std::shared_ptr<MAVConnInterface>;

    
    /**
	 * @param[in] system_id     sysid for send_message
	 * @param[in] component_id  compid for send_message
	 */
	AugMavConnInterface(MAVConnInterface::Ptr mavconnInterface);

    /**
     Codes below is to implement the MAVConnInterface's functions
     WARNING: some of the methods are not implemented
    */
    static AugPtr open_url(std::string url,
			uint8_t system_id = 1, uint8_t component_id = MAV_COMP_ID_UDP_BRIDGE);

    /**
      using the convention interface to test the toolchain
    */
            
    void send_message( mavlink_message_t *message);
        void send_message( mavlink_message_t *message, uint8_t sysid, uint8_t compid);
    void real_send_message(mavlink_message_t message,int id);


    // void send_message(const mavlink_message_t *message,const uint8_t msgid,const uint8_t msglength);
	// void send_message(const mavlink_message_t *message, uint8_t sysid, uint8_t compid,const uint8_t msgid,const uint8_t msglength);
	void send_bytes(const uint8_t *bytes, size_t length);

    MessageSig message_received;
    sig2::signal<void()> port_closed;
    mavlink_status_t get_status();

    inline bool is_open(){
        return stdMavconnInterface->is_open();
    }

    inline uint8_t get_system_id() {
		return stdMavconnInterface->get_system_id();
	}
	inline void set_system_id(uint8_t sysid) {
		stdMavconnInterface->set_system_id(sysid);
	}
	inline uint8_t get_component_id() {
		return stdMavconnInterface->get_component_id();
	}
	inline void set_component_id(uint8_t compid) {
		stdMavconnInterface->set_component_id(compid);
	}
    MAVConnInterface::IOStat get_iostat(){
        return stdMavconnInterface-> get_iostat();
    }
    inline int get_channel() { return stdMavconnInterface->get_channel(); };

    ~AugMavConnInterface();
    void close();

    
    private:
    /**
      FIFO related section
      Message processing section
    */
    /**
    standard MAVConnInterface to hold the underly communication
    */
    MAVConnInterface::Ptr stdMavconnInterface;
    FiFoConnector * conn;
};
};
#endif
